#include "magma_new.h"
#include <stdio.h>

static uint32_t iter_key[32];
	
unsigned char Pi[8][16]=
{
  {1,7,14,13,0,5,8,3,4,15,10,6,9,12,11,2},
  {8,14,2,5,6,9,1,12,15,4,11,0,13,10,3,7},
  {5,13,15,6,9,2,12,10,11,7,8,1,4,3,14,0},
  {7,15,5,10,8,1,6,13,0,9,3,14,11,4,2,12},
  {12,8,2,1,13,4,15,6,7,0,10,5,3,14,9,11},
  {11,3,5,8,2,15,10,13,14,1,7,4,12,9,6,0},
  {6,8,2,3,9,10,5,12,1,14,4,7,11,13,0,15},
  {12,4,6,2,10,5,11,9,14,8,13,7,0,3,15,1}
};


void Magma_Add(const uint32_t *a, const uint32_t *b, uint32_t *c){
    *c = *a ^ *b;
}


void Magma_KeyDeployment(const uint32_t *Key){
    for (int i = 0; i < 8; ++i){
        iter_key[i] = Key[i];
    }

    for (int i = 0; i < 8; ++i){
        iter_key[i+8] = Key[i];
    }

    for (int i = 0; i < 8; ++i){
        iter_key[i+16] = Key[i];
    }

    for (int i = 0; i < 8; ++i){
        iter_key[i+24] = iter_key[9-i-2];
    }
}


void Magma_Add_32(const uint32_t *a, const uint32_t *b, uint32_t *c){
    vec a_byte_form, b_byte_form;
    vec c_byte_form;
    
    uint32_t internal = 0;
    a_byte_form[0] = (uint8_t)((*a & 0xff000000) >> 24);
    a_byte_form[1] = (uint8_t)((*a & 0x00ff0000) >> 16);
    a_byte_form[2] = (uint8_t)((*a & 0x0000ff00) >> 8);
    a_byte_form[3] = (uint8_t)(*a & 0x000000ff);

    b_byte_form[0] = (uint8_t)((*b & 0xff000000) >> 24);
    b_byte_form[1] = (uint8_t)((*b & 0x00ff0000) >> 16);
    b_byte_form[2] = (uint8_t)((*b & 0x0000ff00) >> 8);
    b_byte_form[3] = (uint8_t)(*b & 0x000000ff);
    

    for (int i = 3; i >= 0; --i){
        internal = a_byte_form[i] + b_byte_form[i] + (internal >> 8);
        c_byte_form[i] = internal & 0xff;
    }

    *c = c_byte_form[0];
    *c = (*c_byte_form << 8) + c_byte_form[1];
    *c = (*c_byte_form << 8) + c_byte_form[2];
    *c = (*c_byte_form << 8) + c_byte_form[3];
}


void Magma_t_trans(const uint32_t *in, uint32_t *out){
    vec in_by_byte;
    uint8_t first_part_byte, sec_part_byte;
    vec out_by_byte;

    in_by_byte[0] = (uint8_t)((*in & 0xff000000) >> 24);
    in_by_byte[1] = (uint8_t)((*in & 0x00ff0000) >> 16);
    in_by_byte[2] = (uint8_t)((*in & 0x0000ff00) >> 8);
    in_by_byte[3] = (uint8_t)(*in & 0x000000ff);


    for (int i = 0; i < 4; ++i){
        first_part_byte = (in_by_byte[i] & 0xf0) >> 4;
        sec_part_byte = (in_by_byte[i] & 0x0f);

        first_part_byte = Pi[i*2][first_part_byte];
        sec_part_byte = Pi[i*2 + 1][sec_part_byte];
        out_by_byte[i] = (first_part_byte << 4) | sec_part_byte;
    }

    *out = out_by_byte[0];
    *out = (*out << 8) + out_by_byte[1];
    *out = (*out << 8) + out_by_byte[2];
    *out = (*out << 8) + out_by_byte[3];
}


void Magma_g_trans(const uint32_t *k, const uint32_t *a, uint32_t *res){
    uint32_t internal = 0;

    Magma_Add_32(a, k, &internal);
    Magma_t_trans(&internal, &internal);
    
    internal = (internal << 11) | (internal >> 21);

    *res = internal;
}


void Magma_G_trans(const uint32_t *k, const uint64_t *a, uint64_t *res){
    uint32_t a_0, a_1, G;

    a_0 = *a & 0x00000000ffffffff;
    a_1 = (*a & 0xffffffff00000000) >> 32;

    Magma_g_trans(k, &a_0, &G);
    Magma_Add(&a_1, &G, &G);

    a_1 = a_0;
    a_0 = G;

    *res = a_1;
    *res = (*res << 32) + a_0;
}


void Magma_G_star_trans(const uint32_t *k, const uint64_t *a, uint64_t *res){
    uint32_t  a_0, a_1, G;

    a_0 = *a & 0x00000000ffffffff;
    a_1 = (*a & 0xffffffff00000000) >> 32;

    Magma_g_trans(k, &a_0, &G);
    Magma_Add(&a_1, &G, &G);

    a_1 = G;

    *res = a_1;
    *res = (*res << 32) + a_0;
}


void Magma_Encript(const uint64_t *blk, uint64_t *out){
    *out = 0;
    Magma_G_trans(&(iter_key[0]), blk, out);
    for (int i = 1; i < 31; ++i){
        Magma_G_trans(&(iter_key[i]), out, out);
    }
   
    Magma_G_star_trans(&(iter_key[31]), out, out);
}